#include <math.h>
#include "lamp_detector.h"
#include <iostream>
#include <fstream>

LampDetector::LampDetector(int images_width, int images_height, bool debug) {

  this->debug = debug;

  detection = 0;

  label_space = new int*[images_height];
  for (int i = 0; i < images_height; ++i) {
    label_space[i] = new int[images_width];
  }

  this->images_width = images_width;
  this->images_height = images_height;
  coexistence_matrix = new CoexistenceMatrix(MAX_DETECTABLE_OBJECTS);
  found_objects = new ObjectInImage[MAX_DETECTABLE_OBJECTS];
  nof_found_objects = 0;
}

LampDetector::~LampDetector() {

  for (int i = 0; i < images_height; ++i) {
    delete [] label_space[i];
  }
  delete [] label_space;

  coexistence_matrix->~CoexistenceMatrix();
  delete [] found_objects;
}

bool LampDetector::TestForCircle(int object_to_test_index) {
  ObjectInImage *object = &found_objects[object_to_test_index];
  double center_x = object->sigma_x/object->area;
  double center_y = object->sigma_y/object->area;

  double radius = 0.25*(object->xmax - object->xmin) + 0.25*(object->ymax - object->ymin);

  int anormal_count = 0;

  for (int i = object->xmin; i < object->xmax; ++i) {
    for (int j = object->ymin; j < object->ymax; ++j) {
      bool supposed = (((i - center_x)*(i - center_x) + (j - center_y)*(j - center_y)) <= radius*radius);

      if (supposed xor (label_space[i][j] == object_to_test_index)) anormal_count++;
    }
  }

  if (anormal_count/object->area < 0.2) return true;

  return false;
}

bool LampDetector::TestForRectangle(int object_to_test_index) {
  ObjectInImage *object = &found_objects[object_to_test_index];
  double center_x = object->sigma_x/object->area;
  double center_y = object->sigma_y/object->area;

  double Sxx, Sxy;
  Sxx = Sxy = 0;
  for (int j = object->xmin; j <= object->xmax; j++) {
    for (int k = object->ymin; k <= object->ymax; k++) {
      if (label_space[j][k] == object_to_test_index) {
        Sxx += (j - center_x) * (j - center_x);
        Sxy += (k - center_y) * (j - center_x);
      }
    }
  }
  double a = Sxy / Sxx;

  bool result = false;

  double delta_x = object->xmax - object->xmin;
  double delta_y = object->ymax - object->ymin;

  if (fabs(a) > 6 || fabs(a) < 0.008) {
    double envolving_area = delta_x*delta_y;
    if (object->area/envolving_area > 0.90) {
      double proportion = (delta_x > delta_y) ? delta_x/delta_y : delta_y/delta_x;
      if (proportion > 5 && proportion < 18) result = true;
    }
  } else {
    long a_square = delta_x*delta_x + delta_y*delta_y;

    double b_square;
    if (fabs(a) > 1) {
      long width_count = 0;
      for (int j = object->ymin; j <= object->ymax; ++j) {
        int sup_border, inf_border;
        for (int i = object->xmin; i <= object->xmax; ++i) {
          if (label_space[i][j] == object_to_test_index) { sup_border = i; break; }
        }
        for (int i = object->xmax; i >= object->xmin; --i) {
          if (label_space[i][j] == object_to_test_index) { inf_border = i; break; }
        }
        width_count += (inf_border - sup_border + 1);
      }
      double medium_width = width_count / (delta_y);
      b_square = medium_width*medium_width*a*a/(1 + a*a);
    } else {
      long width_count = 0;
      for (int i = object->xmin; i <= object->xmax; ++i) {
        int sup_border, inf_border;
        for (int j = object->ymin; j <= object->ymax; ++j) {
          if (label_space[i][j] == object_to_test_index) { sup_border = j; break; }
        }
        for (int j = object->ymax; j >= object->ymin; --j) {
          if (label_space[i][j] == object_to_test_index) { inf_border = j; break; }
        }
        width_count += (inf_border - sup_border + 1);
      }
      double medium_width = width_count / (delta_x);
      b_square = medium_width*medium_width/(1 + a*a);
    }

    double proportion_square = a_square/b_square;

    // printf("prop: %lf\n", proportion_square);

    if (proportion_square > 20 && proportion_square < 500) result = true;
  }

  return result;
}

vector<lamp> LampDetector::DetectLamps(int **binarized_image, int min_detectable_area,
    int max_width_variance, int min_width_simetry, double image_scale) {

  coexistence_matrix->ResetDataStructures();

  for (int i = 0; i < images_height; ++i) {
    for (int j = 0; j < images_width; ++j) {
      label_space[i][j] = 0;
    }
  }

  for (int i = 0; i < nof_found_objects; ++i) {
    found_objects[i].Clear();
  }

  // n é o número inicial de objetos detectados, que não é o número real, já que ainda resta a filtragem
  // aqui de fato começa o algoritmo da figura 6.4, página 164, do livro do Davies
  nof_found_objects = 0;
  for (int i = 0; i < images_height; i++) {
    for (int j = 0; j < images_width; j++) {
      if (binarized_image[i][j] == 1) { // se o pixel contém alguma coisa
        // esses p's são os pixels em volta do pixel analisado - na labelSpaceMat -, que é p0
        // p1 é o pixel à direita de p0, os outros se orientam no sentido anti-horário em volta de p0
        int p0, p2, p3, p4, p5;

        // inicializa todos com zero (podem existir pixels fora da imagem, vide a seguir)
        p2 = p3 = p4 = p5 = 0;

        // ...aqui se assegura que todos os pixels existam (o livro abstrai detalhes de programação,
        // portanto não tem essa parte lá) - os que não existirem serão marcados com zero)...
        if (j > 0) p5 = label_space[i][j - 1];

        if (i > 0) {
          if (j < images_width - 1) p2 = label_space[i - 1][j + 1];

          p3 = label_space[i - 1][j];

          if (j > 0) p4 = label_space[i - 1][j - 1];
        }

        //...voltando ao algoritmo...
        if (p2 == 0 && p3 == 0 && p4 == 0 && p5 == 0) { // se esse pixel não tem nenhum vizinho, é um objeto novo
          nof_found_objects++;
          p0 = nof_found_objects; // linha desnecessária, mas tá aí para manter semelhança com o livro
          label_space[i][j] = p0;
        } else { // se ele tem algum vizinho, faz parte do mesmo objeto
          // pega o menor entre os p's maiores do que zero
          p0 = 2147483647;
          if (p2 > 0) p0 = p2;
          if (p3 > 0 && p3 < p0) p0 = p3;
          if (p4 > 0 && p4 < p0) p0 = p4;
          if (p5 > 0 && p5 < p0) p0 = p5;

          if (p2 > 0) coexistence_matrix->Fill(p0, p2);
          if (p3 > 0) coexistence_matrix->Fill(p0, p3);
          if (p4 > 0) coexistence_matrix->Fill(p0, p4);
          if (p5 > 0) coexistence_matrix->Fill(p0, p5);

          label_space[i][j] = p0;
        }
      }
    }
  }

  coexistence_matrix->MinimizeLabels();

  // Corrige os labels, deixando-os iguais aos indicados pela matriz de coexistência
  for (int i = 0; i < images_height; ++i) {
    for (int j = 0; j < images_width; ++j) {
      if (label_space[i][j] != 0) {
        label_space[i][j] = coexistence_matrix->GetCorrectLabel(label_space[i][j]);
      }
    }
  }

  //percorre a imagem mais uma vez, pegando características geométricas dos objetos
  for (int i = 0; i < images_height; i++) {

    ObjectInImage *curntObj = NULL; //marca o índice de um objeto encontrado ao percorrer uma linha

    for (int j = 0; j < images_width; j++) {

      int curntPixelValue = label_space[i][j];

      if (curntPixelValue != 0) {

        if (curntObj == NULL)
          curntObj = &found_objects[curntPixelValue];

        curntObj->area++;
        curntObj->sigma_x += i;
        curntObj->sigma_y += j;
        if (curntObj->xmin > i) curntObj->xmin = i;
        if (curntObj->xmax < i) curntObj->xmax = i;
        if (curntObj->ymin > j) curntObj->ymin = j;
        if (curntObj->ymax < j) curntObj->ymax = j;

      } else if (curntObj != NULL) curntObj = NULL;
    }
  }

  if (debug) {
    ofstream outputImageFile;

    char file_name[21];
    sprintf(file_name, "original%d.pnm", detection);
    outputImageFile.open(file_name);
    outputImageFile << "P2" << endl << images_width << " " << images_height << endl << "1" << endl;
    for (int i = 0; i < images_height; ++i) {
      for (int j = 0; j < images_width; ++j) {
        if (binarized_image[i][j] != 0) outputImageFile << "1 ";
        else outputImageFile << "0 ";
      }
      outputImageFile << endl;
    }
    outputImageFile.close();
  }

  vector<lamp> result;

  for (int i = 0; i < nof_found_objects; i++) {
    ObjectInImage *curntObject = &found_objects[i];
    if (curntObject->area > min_detectable_area) {
      bool is_lamp = false;

      int delta_x = curntObject->xmax - curntObject->xmin;
      int delta_y = curntObject->ymax - curntObject->ymin;
      double proportion = (delta_x >= delta_y) ? delta_x/delta_y : delta_y/delta_x;
      if (proportion > 0.9 && proportion < 1.2) {
        if (!(is_lamp = TestForCircle(i))) is_lamp = TestForRectangle(i);
      } else {
        is_lamp = TestForRectangle(i);
      }

      if (is_lamp) {
        curntObject->used = true;

        lamp detected;
        detected.x = image_scale*((double)curntObject->sigma_y/curntObject->area - (double) images_height/2);
        detected.y = image_scale*((double)curntObject->sigma_x/curntObject->area - (double) images_width/2);

        result.push_back(detected);
      }
    }
  }

  if (debug) {
    ofstream outputImageFile;

    char file_name[21];
    sprintf(file_name, "filtrada%d.pnm", detection);
    outputImageFile.open(file_name);
    detection++;
    outputImageFile << "P2" << endl << images_width << " " << images_height << endl << "1" << endl;
    for (int i = 0; i < images_height; ++i) {
      for (int j = 0; j < images_width; ++j) {
        if (found_objects[label_space[i][j]].used) outputImageFile << "1 ";
        else outputImageFile << "0 ";
      }
      outputImageFile << endl;
    }
    outputImageFile.close();
  }

  return result;
}
